﻿using System;
/// 导航计算向量数据结构类
namespace ShawnNav
{
    public struct NavVector3
    {
        public float x;
        public float y;
        public float z;

        #region 构造函数
        public NavVector3(float x, float y, float z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public NavVector3(float x, float z)
        {
            this.x = x;
            this.y = 0;
            this.z = z;
        }
        #endregion

        #region 常用功能
        public static NavVector3 Zero
        {
            get { return new NavVector3(0, 0, 0); }
        }
        public static NavVector3 One
        {
            get { return new NavVector3(1, 1, 1); }
        }

        public static NavVector3 operator +(NavVector3 a, NavVector3 b)
        {
            return new NavVector3(a.x + b.x, a.y + b.y, a.z + b.z);
        }
        public static NavVector3 operator -(NavVector3 a, NavVector3 b)
        {
            return new NavVector3(a.x - b.x, a.y - b.y, a.z - b.z);
        }
        public static NavVector3 operator *(NavVector3 a, float val)
        {
            return new NavVector3(a.x * val, a.y * val, a.z * val);
        }
        public static NavVector3 operator /(NavVector3 a, float val)
        {
            return new NavVector3(a.x / val, a.y / val, a.z / val);
        }
        public static bool operator ==(NavVector3 a, NavVector3 b)
        {
            return a.x == b.x && a.y == b.y && a.z == b.z;
        }
        public static bool operator !=(NavVector3 a, NavVector3 b)
        {
            return a.x != b.x || a.y != b.y || a.z != b.z;
        }
        public static float DotXZ(NavVector3 a, NavVector3 b)
        {
            return a.x * b.x + a.z * b.z;
        }
        public static float CrossXZ(NavVector3 a, NavVector3 b)
        {
            return a.x * b.z - a.z * b.x;
        }
        public static NavVector3 Cross(NavVector3 a, NavVector3 b)
        {
            return new NavVector3()
            {
                x = a.y * b.z - a.z * b.y,
                y = a.z * b.x - a.x * b.z,
                z = a.x * b.y - a.y * b.x
            };
        }
        public static NavVector3 NormalXZ(NavVector3 v)
        {
            float len = MathF.Sqrt(v.x * v.x + v.z * v.z);
            NavVector3 nor = new NavVector3
            {
                x = v.x / len,
                y = 0,
                z = v.z / len
            };
            return nor;
        }
        public static float AngleXZ(NavVector3 a, NavVector3 b)
        {
            float dot = DotXZ(a, b);
            float angle = MathF.Acos(dot);
            NavVector3 cross = Cross(a, b);
            if (cross.y < 0)
            {
                angle = -angle;
            }
            return angle;
        }
        public static float SqrDistance(NavVector3 a, NavVector3 b)
        {
            return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z);
        }
        public override bool Equals(object obj)
        {
            return obj is NavVector3 vector &&
                x == vector.x &&
                y == vector.y &&
                z == vector.z;
        }
        public override int GetHashCode()
        {
            return HashCode.Combine(x, y, z);
        }
        public override string ToString()
        {
            return $"[{x}_{z}]";
        }
        #endregion
    }
}
